/*******************************************************************************
 *     Description: MAXQ622 Autonomous Car
 *             $Id$
 *******************************************************************************
 */

#include "maxq_config.h"
#include "watchdog/maxq_watchdog.h"
#include "comm/maxq_comm.h"
#include "tick.h"
#include "dist.h"
#include "compass.h"

/***** Definitions *****/
#define RELOAD_VAL      (DEFAULT_SYS_CLK_FREQ / 800)  // (sys clk / 64) / 12.5 ticks per second
#define OVERFLOW_FLAG   0x80    // bit mask for TBnCN.7:TFB

/***** Global Data *****/
int compass_exists;

/***** Function Prototypes *****/
void tick_init(void);
extern void navif_init(void);

/****************************************************************************/
int main( void )
{
  unsigned int heading;
  int i;

  compass_exists = 0;

  comm_init();
  tick_init();

  // Configure port for heartbeat
  PD3 |= 0x03;
  PO3 |= 0x01;
  PO3 &= ~0x02;

  maxq_printf(C"\r\n\r\n***** I2C Sensors *****\r\n");

  if (!dist_init())
    maxq_printf("Failed to initialize distance sensor(s)\r\n");

#if 0
  maxq_printf("Changing Ultrasonic Sensor ID...\r\n");
  if (!dist_set_id(SENSOR_ADDR_BASE, SENSOR_ADDR(2)))
    maxq_printf("Failed to change ID\r\n");
#endif

  if (!compass_init())
    maxq_printf("Failed to initialize compass\r\n");
  else
    compass_exists = 1;

  __enable_interrupt();

  while(1) {
    if (flag_100ms) {
      flag_100ms = 0;

      dist_task();
    }

    if (flag_200ms) {
      flag_200ms = 0;
      PO3 ^= 0x03;  // heartbeat

#if 1
      if (compass_exists && !compass_read(&heading))
        maxq_printf("Compass read failed\r\n");

      maxq_printf("Distance:");
      for (i = 0; i < NUM_SENSORS; i++)
        maxq_printf(" %4d cm", distance[i]);

      if (compass_exists) {
        maxq_printf(", Heading: %d.%d\r\n", heading / 10, heading % 10);
      }
      maxq_printf("\r\n");
#endif
    }
  }
}
